using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using static BepuUtilities.GatherScatter;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints
{

    /// <summary>
    /// 将两个实体上的点约束为按目标距离分隔。
    /// </summary>
    public struct DistanceServo : ITwoBodyConstraintDescription<DistanceServo>
    {
        /// <summary>
        /// 从主体A的中心到其附着点的局部偏移。
        /// </summary>
        public Vector3 LocalOffsetA;
        /// <summary>
        /// 从主体B的中心到其附着点的局部偏移。
        /// </summary>
        public Vector3 LocalOffsetB;
        /// <summary>
        /// 约束将尝试在附着点之间达到的距离。
        /// </summary>
        public float TargetDistance;
        /// <summary>
        /// 伺服控制参数。
        /// </summary>
        public ServoSettings ServoSettings;
        /// <summary>
        /// 弹簧频率和阻尼参数。
        /// </summary>
        public SpringSettings SpringSettings;

        /// <summary>
        /// 创建距离伺服描述。
        /// </summary>
        /// <param name="localOffsetA">正文A中心到其附着点的局部偏移。</param>
        /// <param name="localOffsetB">正文B中心到其附着点的局部偏移。</param>
        /// <param name="targetDistance">约束将尝试在附着点之间达到的距离。</param>
        /// <param name="springSettings">弹簧频率和阻尼参数。</param>
        /// <param name="servoSettings">伺服控制参数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DistanceServo(in Vector3 localOffsetA, in Vector3 localOffsetB, float targetDistance, in SpringSettings springSettings, in ServoSettings servoSettings)
        {
            LocalOffsetA = localOffsetA;
            LocalOffsetB = localOffsetB;
            TargetDistance = targetDistance;
            SpringSettings = springSettings;
            ServoSettings = servoSettings;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DistanceServo(in Vector3 localOffsetA, in Vector3 localOffsetB, float targetDistance, in SpringSettings springSettings)
            : this(localOffsetA, localOffsetB, targetDistance, springSettings, ServoSettings.Default)
        {
        }

        public int ConstraintTypeId
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return DistanceServoTypeProcessor.BatchTypeId;
            }
        }

        public Type TypeProcessorType => typeof(DistanceServoTypeProcessor);

        public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
        {
            Debug.Assert(TargetDistance >= 0, "DistanceServo.TargetDistance must be nonnegative.");
            ConstraintChecker.AssertValid(ServoSettings, SpringSettings, nameof(DistanceServo));
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var target = ref GetOffsetInstance(ref Buffer<DistanceServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Vector3Wide.WriteFirst(LocalOffsetA, ref target.LocalOffsetA);
            Vector3Wide.WriteFirst(LocalOffsetB, ref target.LocalOffsetB);
            GatherScatter.GetFirst(ref target.TargetDistance) = TargetDistance;
            ServoSettingsWide.WriteFirst(ServoSettings, ref target.ServoSettings);
            SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings);
        }

        public void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out DistanceServo description)
        {
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var source = ref GetOffsetInstance(ref Buffer<DistanceServoPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Vector3Wide.ReadFirst(source.LocalOffsetA, out description.LocalOffsetA);
            Vector3Wide.ReadFirst(source.LocalOffsetB, out description.LocalOffsetB);
            description.TargetDistance = GatherScatter.GetFirst(ref source.TargetDistance);
            ServoSettingsWide.ReadFirst(source.ServoSettings, out description.ServoSettings);
            SpringSettingsWide.ReadFirst(source.SpringSettings, out description.SpringSettings);
        }
    }

    public struct DistanceServoPrestepData
    {
        public Vector3Wide LocalOffsetA;
        public Vector3Wide LocalOffsetB;
        public Vector<float> TargetDistance;
        public ServoSettingsWide ServoSettings;
        public SpringSettingsWide SpringSettings;
    }

    public struct DistanceServoProjection
    {
        public Vector3Wide LinearVelocityToImpulseA;
        public Vector3Wide AngularVelocityToImpulseA;
        public Vector3Wide AngularVelocityToImpulseB;
        public Vector<float> BiasImpulse;
        public Vector<float> SoftnessImpulseScale;
        public Vector<float> MaximumImpulse;
        public Vector3Wide LinearImpulseToVelocityA;
        public Vector3Wide AngularImpulseToVelocityA;
        public Vector3Wide LinearImpulseToVelocityB;
        public Vector3Wide AngularImpulseToVelocityB;
    }

    public struct DistanceServoFunctions : IConstraintFunctions<DistanceServoPrestepData, DistanceServoProjection, Vector<float>>
    {
        public static void GetDistance(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, in Vector3Wide localOffsetA, in Vector3Wide localOffsetB,
            out Vector3Wide anchorOffsetA, out Vector3Wide anchorOffsetB, out Vector3Wide anchorOffset, out Vector<float> distance)
        {
            bodies.GatherPose(ref bodyReferences, count, out var offsetB, out var orientationA, out var orientationB);

            QuaternionWide.TransformWithoutOverlap(localOffsetA, orientationA, out anchorOffsetA);
            QuaternionWide.TransformWithoutOverlap(localOffsetB, orientationB, out anchorOffsetB);
            Vector3Wide.Add(anchorOffsetB, offsetB, out var anchorB);
            Vector3Wide.Subtract(anchorB, anchorOffsetA, out anchorOffset);

            Vector3Wide.Length(anchorOffset, out distance);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ComputeTransforms(
            in BodyInertias inertiaA, in BodyInertias inertiaB, in Vector3Wide anchorOffsetA, in Vector3Wide anchorOffsetB, in Vector<float> distance, ref Vector3Wide direction,
            float dt, in SpringSettingsWide springSettings,
            out Vector<float> positionErrorToVelocity, out Vector<float> softnessImpulseScale, out Vector<float> effectiveMass,
            out Vector3Wide linearVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseA, out Vector3Wide angularVelocityToImpulseB,
            out Vector3Wide linearImpulseToVelocityA, out Vector3Wide angularImpulseToVelocityA, out Vector3Wide linearImpulseToVelocityB, out Vector3Wide angularImpulseToVelocityB)
        {
            // 位置约束：
            // |PositionA+localOffsetA*OrientationA-Position B-localOffsetB*OrientationB||=距离
            // 跳过一串代数,速度约束适用于沿分离轴的速度变化。
            // 点(线性A+角度A x(localOffsetA*OrientationA)-线性B角度A x(localOffsetB*OrientationB),规格化(Position A+localOffsetA*OrientationA-Position B-localOffsetB*OrientationB))=0
            // 点(线性A,方向)+点(角度A x偏移A,方向)+点(线性B,-方向)+点(角度B x偏移B,-方向)=0
            // 点(线性A,方向)+点(偏移A x方向,角度A)+点(线性B,-方向)+点(偏移B x方向,角度B)=0
            // 点(线性A,方向)+点(偏移A x方向,角度A)-点(线性B,方向)+点(方向x偏移B,角度B)=0
            // 雅可比是方向、-方向、偏移量A x方向和方向x偏移量B。
            // 这是9个独特的标量。
            // 我们可以把这9加14储存起来,作为逆质量,或者我们可以预乘。
            // V*JT*Me*J*I^-1
            // 如果将JT*Me和J*i^-1预乘,速度->脉冲的标量为9,脉冲->速度的标量为12。
            // 如果不预乘,雅克比是9,反惯性是14,有效质量是1。
            // 那是21对24。最重要的是,预乘节省了一些ALU工作。

            // 请注意,我们使用的是距离,而不是距离的平方。这使得它更容易使用和推理,在前一步中以平方根为代价。
            // 这真的,真的无关紧要。

            // 如果距离为零,则没有有效的偏移方向。随便挑一个。
            var needFallback = Vector.LessThan(distance, new Vector<float>(1e-9f));
            direction.X = Vector.ConditionalSelect(needFallback, Vector<float>.One, direction.X);
            direction.Y = Vector.ConditionalSelect(needFallback, Vector<float>.Zero, direction.Y);
            direction.Z = Vector.ConditionalSelect(needFallback, Vector<float>.Zero, direction.Z);

            Vector3Wide.CrossWithoutOverlap(anchorOffsetA, direction, out var angularJA);
            Vector3Wide.CrossWithoutOverlap(direction, anchorOffsetB, out var angularJB); // 注意翻转否定。

            // 线性雅可比贡献是标量乘以1,因为它是单位长度矢量。
            Symmetric3x3Wide.VectorSandwich(angularJA, inertiaA.InverseInertiaTensor, out var angularContributionA);
            Symmetric3x3Wide.VectorSandwich(angularJB, inertiaB.InverseInertiaTensor, out var angularContributionB);
            var inverseEffectiveMass = inertiaA.InverseMass + inertiaB.InverseMass + angularContributionA + angularContributionB;

            SpringSettingsWide.ComputeSpringiness(springSettings, dt, out positionErrorToVelocity, out var effectiveMassCFMScale, out softnessImpulseScale);
            effectiveMass = effectiveMassCFMScale / inverseEffectiveMass;
            Vector3Wide.Scale(direction, effectiveMass, out linearVelocityToImpulseA);
            Vector3Wide.Scale(angularJA, effectiveMass, out angularVelocityToImpulseA);
            Vector3Wide.Scale(angularJB, effectiveMass, out angularVelocityToImpulseB);

            Vector3Wide.Scale(direction, inertiaA.InverseMass, out linearImpulseToVelocityA);
            Symmetric3x3Wide.TransformWithoutOverlap(angularJA, inertiaA.InverseInertiaTensor, out angularImpulseToVelocityA);
            Vector3Wide.Scale(direction, -inertiaB.InverseMass, out linearImpulseToVelocityB);
            Symmetric3x3Wide.TransformWithoutOverlap(angularJB, inertiaB.InverseInertiaTensor, out angularImpulseToVelocityB);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Prestep(Bodies bodies, ref TwoBodyReferences bodyReferences, int count, float dt, float inverseDt, ref BodyInertias inertiaA, ref BodyInertias inertiaB,
            ref DistanceServoPrestepData prestep, out DistanceServoProjection projection)
        {
            GetDistance(bodies, ref bodyReferences, count, prestep.LocalOffsetA, prestep.LocalOffsetB, out var anchorOffsetA, out var anchorOffsetB, out var anchorOffset, out var distance);

            Vector3Wide.Scale(anchorOffset, Vector<float>.One / distance, out var direction);

            ComputeTransforms(inertiaA, inertiaB, anchorOffsetA, anchorOffsetB, distance, ref direction, dt, prestep.SpringSettings,
                out var positionErrorToVelocity, out projection.SoftnessImpulseScale, out var effectiveMass,
                out projection.LinearVelocityToImpulseA, out projection.AngularVelocityToImpulseA, out projection.AngularVelocityToImpulseB,
                out projection.LinearImpulseToVelocityA, out projection.AngularImpulseToVelocityA, out projection.LinearImpulseToVelocityB, out projection.AngularImpulseToVelocityB);

            // 计算位置误差和偏置速度。计算误差时请注意减法的顺序-我们希望偏置速度抵消分离。
            var error = distance - prestep.TargetDistance;
            ServoSettingsWide.ComputeClampedBiasVelocity(error, positionErrorToVelocity, prestep.ServoSettings, dt, inverseDt, out var clampedBiasVelocity, out projection.MaximumImpulse);
            projection.BiasImpulse = clampedBiasVelocity * effectiveMass;

        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB,
            in Vector3Wide linearImpulseToVelocityA, in Vector3Wide angularImpulseToVelocityA, in Vector3Wide linearImpulseToVelocityB, in Vector3Wide angularImpulseToVelocityB,
            ref Vector<float> csi)
        {
            Vector3Wide.Scale(linearImpulseToVelocityA, csi, out var linearVelocityChangeA);
            Vector3Wide.Scale(angularImpulseToVelocityA, csi, out var angularVelocityChangeA);
            Vector3Wide.Add(linearVelocityChangeA, velocityA.Linear, out velocityA.Linear);
            Vector3Wide.Add(angularVelocityChangeA, velocityA.Angular, out velocityA.Angular);
            Vector3Wide.Scale(linearImpulseToVelocityB, csi, out var linearVelocityChangeB);
            Vector3Wide.Scale(angularImpulseToVelocityB, csi, out var angularVelocityChangeB);
            Vector3Wide.Add(linearVelocityChangeB, velocityB.Linear, out velocityB.Linear);
            Vector3Wide.Add(angularVelocityChangeB, velocityB.Angular, out velocityB.Angular);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DistanceServoProjection projection, ref Vector<float> accumulatedImpulse)
        {
            ApplyImpulse(ref velocityA, ref velocityB,
                projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref accumulatedImpulse);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref DistanceServoProjection projection, ref Vector<float> accumulatedImpulse)
        {
            // CSI=投射.BiasImPulse-累加脉冲*投射.SoftnessImPulseScale-(csia线性+csiaAngular+csib线性+csibAngular);
            Vector3Wide.Dot(velocityA.Linear, projection.LinearVelocityToImpulseA, out var linearCSIA);
            Vector3Wide.Dot(velocityB.Linear, projection.LinearVelocityToImpulseA, out var negatedLinearCSIB);
            Vector3Wide.Dot(velocityA.Angular, projection.AngularVelocityToImpulseA, out var angularCSIA);
            Vector3Wide.Dot(velocityB.Angular, projection.AngularVelocityToImpulseB, out var angularCSIB);
            var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - (linearCSIA + angularCSIA - negatedLinearCSIB + angularCSIB);
            ServoSettingsWide.ClampImpulse(projection.MaximumImpulse, ref accumulatedImpulse, ref csi);

            ApplyImpulse(ref velocityA, ref velocityB,
                projection.LinearImpulseToVelocityA, projection.AngularImpulseToVelocityA, projection.LinearImpulseToVelocityB, projection.AngularImpulseToVelocityB, ref csi);
        }

    }


    /// <summary>
    /// 处理一组距离伺服的求解迭代。
    /// </summary>
    public class DistanceServoTypeProcessor : TwoBodyTypeProcessor<DistanceServoPrestepData, DistanceServoProjection, Vector<float>, DistanceServoFunctions>
    {
        public const int BatchTypeId = 33;
    }
}
